
import lejos.robotics.subsumption.*;
import lejos.nxt.*;
import lejos.robotics.navigation.*;

/**
 * Simple code that implements the bug 2 algorithm to find path to the goal
 * which is light sensor. It uses three different behavior:
 * 1. find goal
 * 2. go to goal
 * 3. avoid obstacles
 * 
 * @author Okan Aşık
 *
 */
public class PathPlanning
{
	static boolean goalFound = false;
	static int xGoal = 50;
	static int yGoal = 200;
	
	static boolean quickTest = true;
	
  public static void main(String[] args)
  {
    Motor.A.setSpeed(200);
    Motor.C.setSpeed(200);
    
    Behavior findGoal = new FindGoal();
    Behavior gotoGoal = new GotoGoal();
    Behavior avoidObstacle = new AvoidObstacle();
    
    Behavior[] behaviorList =
    {
    		gotoGoal, avoidObstacle, findGoal
    };
    
    Arbitrator arbitrator = new Arbitrator(behaviorList);
    LCD.drawString("Bug 1", 0, 0);
    Button.waitForPress();
    arbitrator.start();
  }
}


class FindGoal implements Behavior
{
	private LightSensor lightSensor = new LightSensor(SensorPort.S2); 
	private static boolean goalFound = false;
	
	//according problem definition it is sufficient to know the position
	//in terms of x and y...
	public static int xDistance = 0;
	public static int yDistance = 0;
	public static int angle = 0;

	public static TachoPilot robot = new TachoPilot(2.17f,4.57f,Motor.A, Motor.C);
  
	public FindGoal() {
		lightSensor.setFloodlight(false);
	}
	
	public boolean takeControl()
	{
		return !goalFound;  // this behavior always wants control.
	}

	public void suppress()
	{
		//_suppressed = true;
		// standard practice for suppress methods
	}

	public void action()
	{
		LCD.drawString("Finding Goal...", 0, 1);
		
		int maxLight = -1;
		int delta = 5;
		
		//find the maximum light intensity
		for (int i=0; i<=360;) {
			int lightValue = lightSensor.readValue();
			
			if (lightValue > maxLight) {
				maxLight = lightValue;
				angle = i;
			}
			
			try {
				robot.rotate(delta);
				Thread.sleep(300);
				i+=delta;
			}
			catch (InterruptedException ex) {
				
			}
		}
		
		//turn to the find angle
		  if (angle > 180)
			  FindGoal.robot.rotate((-1)* (360 - angle));
		  else 
			  FindGoal.robot.rotate(angle);
		  
		goalFound = true;
		
		robot.reset();
	}
}

class GotoGoal implements Behavior
{

  public GotoGoal()
  {
	  
  }

  //always tries to take control to go to the goal
  public boolean takeControl()
  {
	  return true;
  }

  public void suppress()
  {
	  FindGoal.robot.stop();
	  
	  //calculate total distance when going forward is suppressed
	  //going forward will be suppressed if the robot hits to the obstacle 
	  FindGoal.yDistance += (int)Math.round(Math.cos(Math.toRadians(FindGoal.angle)) * FindGoal.robot.getTravelDistance());
	  FindGoal.xDistance += (int)Math.round(Math.sin(Math.toRadians(FindGoal.angle)) * FindGoal.robot.getTravelDistance());
  }

  public void action()
  {
	  LCD.drawString("Going to Goal...", 0, 1);
	  FindGoal.robot.forward();
	  
	  int yTravel = (int)Math.round(Math.sin(Math.toRadians(FindGoal.angle)) * FindGoal.robot.getTravelDistance());
	  int xTravel = (int)Math.round(Math.cos(Math.toRadians(FindGoal.angle)) * FindGoal.robot.getTravelDistance());
	  
	  if ((FindGoal.xDistance + xTravel) >= PathPlanning.xGoal && (FindGoal.yDistance + yTravel) >= PathPlanning.yGoal) {
		  FindGoal.robot.stop();
	  }
  }
  
}

class AvoidObstacle implements Behavior
{
	private static boolean obstacleDetected = false; 
	private static boolean turnRight = true;
	
	public static TouchSensor touchSensor = new TouchSensor(SensorPort.S4);
	public static UltrasonicSensor leftSensor = new UltrasonicSensor(SensorPort.S1);
		
  public AvoidObstacle()
  {
	  
  }

  public boolean takeControl()
  {
	  return touchSensor.isPressed();
  }

  public void suppress()
  {
    //Since  this is highest priority behavior, suppress will never be called.
  }

  public void action()
  {
	  int delta = 3;
	  int threshold = 3;
	  int backoff = 10;
	  int angle = 0;
	  int vehicleLength = 20;
	  
	  LCD.drawString("Avoiding Obstacle.....", 0, 1);
	  
	  if (!obstacleDetected) {
		  obstacleDetected = true;
	  }
	  
	  //back off for 10 centimeters
	  FindGoal.robot.travel((-1)*backoff);
	  
	  //assume hitting did not change the direction of the robot
	  //remove the backoff value from the position vector
	  FindGoal.yDistance -= (int)Math.round(Math.cos(Math.toRadians(FindGoal.angle)) * backoff);
	  FindGoal.xDistance -= (int)Math.round(Math.sin(Math.toRadians(FindGoal.angle)) * backoff);
	  
	  //back off
	  //while backing off 
	  //control ultrasonic sensor variance, if variance is negative 
	  //which means that we turned our robot parallel to the obstacle
	  
	  //assume that obstacles are rectangle and aligned vertically with the environment
	  //reset the tacho meter to get the turning angle
	  FindGoal.robot.reset();
	  
	  for (int i=0; i<360;  ) {
			  
		  int variance = 0;
		  int value = leftSensor.getDistance();
		  
		  try {
			  FindGoal.robot.rotate(delta);
			  Thread.sleep(500);
			  i += delta;
		  }
		  catch (InterruptedException ex) {
			  
		  }
		  
		  variance = value - leftSensor.getDistance();
		  
		  //if there is no difference between read value from ultrasonic sensor than we are turned to correct position
		  if (variance  < 0) {
			  
			  //set total turn of the robot
			  angle = i - (2*delta);
			  FindGoal.robot.rotate(-(2*delta));
			  break;
		  }
	  }
	  
	  //update the orientation of the robot according to first angle of the robot
	  FindGoal.angle += FindGoal.robot.getAngle();
	  
	  LCD.drawString("backing off is OK", 0, 2);
	  
	  //we correctly positioned the robot now we can circumvent the obstacle
	  int distance = leftSensor.getDistance();
	  LCD.drawString("distance: " + distance, 0, 2);
	  
	  //while we see the wall we go straight
	  //reset to calculate traveled distance in x direction
	  FindGoal.robot.reset();
	  
	  while (distance - leftSensor.getDistance() >= (-1*threshold) &&  distance - leftSensor.getDistance() <= threshold && !(leftSensor.getDistance() == 255)) {
		  FindGoal.robot.travel(delta);
	  }
	  
	  //go as the vehicle length to be able to turn the corner
	  FindGoal.robot.travel(vehicleLength);
	  //add x distance travel
	  FindGoal.xDistance += FindGoal.robot.getTravelDistance();
	  
	  //turn 90 degree to circumvent the obstacle
	  FindGoal.robot.rotate(90);
	  
	  //before going in this direction lets check 
	  //where is the robot and is there a direct path to the goal
	  if (FindGoal.xDistance >= PathPlanning.xGoal) {
	  
		  distance = leftSensor.getDistance();
		  //while we see the wall we go straight
		  //reset to calculate traveled distance in x direction
		  FindGoal.robot.reset();
		  while (distance - leftSensor.getDistance() >= (-1*threshold) &&  distance - leftSensor.getDistance() <= threshold && !(leftSensor.getDistance() == 255)) {
			  FindGoal.robot.travel(delta);
		  }
		  
		  FindGoal.yDistance += FindGoal.robot.getTravelDistance();
		  
	  }
	  
	  //we can direct robot towards goal and goto it.
	  //calculate angle to turn to the goal
	  angle = calculateAngleToGoal();
	  
	  //turn to the goal
	  if (angle > 180)
		  FindGoal.robot.rotate((-1)* (360 - angle));
	  else 
		  FindGoal.robot.rotate(angle);
	  
	  FindGoal.robot.reset();
  }
  
  private int calculateAngleToGoal() {
	  return (int)Math.round(Math.toDegrees(Math.atan((PathPlanning.xGoal - FindGoal.xDistance)/(PathPlanning.yGoal - FindGoal.yDistance))));
  }
}


